#ifndef _ROBOT_CROBOTPOSE_H_
#define _ROBOT_CROBOTPOSE_H_

#include <robot/robot.h>
#include <buola/mat.h>
#include <buola/utility/placeholders.h>
#include <buola/mat/cquat.h>

namespace buola { namespace robot {

template<typename tKinematics>
class CRobotPose
{
public:
    explicit CRobotPose(const tKinematics &pKinematics=tKinematics())
        :   mKinematics(pKinematics)
        ,   mJoints(pKinematics.GetNumJoints(),0.0)
    {}

    CRobotPose(const CRobotPose &pPose)
        :   mKinematics(pPose.mKinematics)
        ,   mJoints(pPose.mJoints)
    {}

    explicit CRobotPose(const double *pJoints,const tKinematics &pKinematics=tKinematics())
        :   mKinematics(pKinematics)
        ,   mJoints(pJoints,pJoints+pKinematics.GetNumJoints())
    {}

    explicit CRobotPose(const std::vector<double> &pJoints,const tKinematics &pKinematics=tKinematics())
        :   mKinematics(pKinematics)
        ,   mJoints(pJoints)
    {}

    template<size_t N>
    explicit CRobotPose(const mat::CVecNd<N> &pJoints,const tKinematics &pKinematics=tKinematics())
        :   mKinematics(pKinematics)
    {
        mJoints.resize(pKinematics.GetNumJoints());
        for(int i=0;i<pKinematics.GetNumJoints();i++)
            mJoints[i]=pJoints[i];
    }

    int GetNumJoints() const {  return mKinematics.GetNumJoints();   }

    double &operator[](int pJ)              {   return mJoints[pJ]; }
    const double &operator[](int pJ) const  {   return mJoints[pJ]; }

    std::vector<double> &GetJoints()        {   return mJoints;     }
    const std::vector<double> &GetJoints() const        {   return mJoints;     }

    const tKinematics &GetKinematics() const  {   return mKinematics;  }

    void InitLinkMatrix(int pLink,mat::CMat34d &pMatrix) const
    {
        double lTheta=mKinematics.GetTheta(pLink)+((pLink>0)?mJoints[pLink-1]:0.0);
        double lD=mKinematics.GetD(pLink);
        double lA=mKinematics.GetA(pLink);
        double lAlpha=mKinematics.GetAlpha(pLink);
        pMatrix(0,0)=std::cos(lTheta);
        pMatrix(0,1)=       -std::sin(lTheta)*std::cos(lAlpha);
        pMatrix(0,2)=               std::sin(lTheta)*std::sin(lAlpha);
        pMatrix(0,3)=                       lA*std::cos(lTheta);
        pMatrix(1,0)=std::sin(lTheta);
        pMatrix(1,1)=       std::cos(lTheta)*std::cos(lAlpha);
        pMatrix(1,2)=               -std::cos(lTheta)*std::sin(lAlpha);
        pMatrix(1,3)=                       lA*std::sin(lTheta);
        pMatrix(2,0)=0;
        pMatrix(2,1)=       std::sin(lAlpha);
        pMatrix(2,2)=               std::cos(lAlpha);
        pMatrix(2,3)=                       lD;
    }

private:
    tKinematics mKinematics;
    std::vector<double> mJoints;
};

template<typename tKinematics>
inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,
                    const CRobotPose<tKinematics> &pP)
{
    for(int i=0;i<pP.GetNumJoints();i++)
    {
        pWriter << pP[i] << ' ';
        pWriter << "\n";
    }
    return pWriter;
}

template<typename tKinematics>
void forward_kinematics(const CRobotPose<tKinematics> &pPose,mat::CVec3d &pPos,
                                mat::CQuat_d &pRot,int pJoint=-1)
{
    mat::CMat34d lMatrix{1,0,0,0,0,1,0,0,0,0,1,0};
    mat::CMat34d lNewMatrix;

    int lMatrices=1+(pJoint==-1?pPose.GetNumJoints():pJoint);

    for(int i=0;i<lMatrices;++i)
    {
        pPose.InitLinkMatrix(i,lNewMatrix);

        lMatrix=lMatrix*lNewMatrix;
    }

    pPos=lMatrix*mat::CVec3d(0,0,0);
    pRot=mat::CQuat_d(lMatrix);
}

template<typename tKinematics>
void forward_kinematics(const CRobotPose<tKinematics> &pPose,mat::CMat34d &pMat,
                                int pJoint=-1)
{
    mat::CMat34d lMatrix;
    mat::CMat34d lNewMatrix;

    int lMatrices=1+(pJoint==-1?pPose.GetNumJoints():pJoint);

    for(int i=0;i<lMatrices;++i)
    {
        pPose.InitLinkMatrix(i,lNewMatrix);

        lMatrix=lMatrix*lNewMatrix;
    }

    pMat=lMatrix;
}

template<typename tKinematics>
void inverse_kinematics(const mat::CVec3d &pPos,const mat::CQuat_d &pRot,std::vector<CRobotPose<tKinematics> > &pDst,
                    const tKinematics &pKinematics=tKinematics())
{
    pKinematics.SolveIK(pDst,pPos,pRot);
}

template<typename tKinematics>
void inverse_kinematics(const mat::CVec3d &pPos,const mat::CQuat_d &pRot,
                        const CRobotPose<tKinematics> &pRef,CRobotPose<tKinematics> &pDst)
{
    std::vector<CRobotPose<tKinematics> > lAll;
    inverse_kinematics(pPos,pRot,lAll,pRef.GetKinematics());
    std::sort(lAll.begin(),lAll.end(),std::bind(tKinematics::ComparePoses,_1,_2,std::ref(pRef)));

    if(lAll.empty())
        throw XNotFound("can't find any joint configuration in inverse_kinematics");

    pDst=lAll[0];
}

/*namespace robot*/ } /*namespace buola*/ }

#endif
